/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package javatest1;

/**
 *
 * @author paul
 */
public class RoboDrive extends RoboTask {
    // simulated position
    double heading = 0;
    double xSpeed = 0;
    double ySpeed = 0;
    double rotSpeed;
    double x = 0;
    double y = 0;
    double width = 27;
    double length = 48;
    double gyroZero = 0;
    private double ySpeedScale = 80.0;   // max speed in/sec
    private double xSpeedScale = 50.0;   // max speed in/sec
    private double rotSpeedScale = 180.0; // max rot deg/sec
    private double lastSimTime = 0;

    void go(double ahead, double right, double turn){
        ySpeed = ahead;
        xSpeed = right;
        rotSpeed = turn;
    }

    double readGyro(){
        return (heading - gyroZero);
    }
    void resetGyro(){
        gyroZero = heading;
    }

    void stop(){
        super.stop();
        ySpeed = 0;
        xSpeed = 0;
        rotSpeed = 0;
    }

    // note: called about 20 times per second
    void simulate(){
        double dt = robot.simDt;
        x = x + Math.cos(Math.toRadians(heading)) * xSpeed  * xSpeedScale * dt + Math.sin(Math.toRadians(heading)) * ySpeed * ySpeedScale * dt;
        y = y + Math.sin(Math.toRadians(heading)) * xSpeed * xSpeedScale * dt + Math.cos(Math.toRadians(heading)) * ySpeed * ySpeedScale * dt;
        heading += rotSpeed * rotSpeedScale * dt;
    }
}
